
#include "User_define.h"
#include <math.h>


//CXL10GP3 Accelerometer


////CRS03-04 Gyroscope

void CRS0304_zeroCalibrate(int totSamples)
{
     double xyz[3];
	 float tmpOffsets[]={0,0,0};
	 for(int i=0;i<totSamples;i++)
	 {
	       
		   CRS0304_readGyroRaw1(xyz);
		   
	       tmpOffsets[0]+=xyz[0];
		   tmpOffsets[1]+=xyz[1];
		   tmpOffsets[2]+=xyz[2];

      }
	  CRS0304_setOffsets(-tmpOffsets[0]/totSamples,-tmpOffsets[1]/totSamples,-tmpOffsets[2]/totSamples);

}

void CRS0304_readGyroRaw(double *_GyroX,double *_GyroY,double *_GyroZ);
{

      *_GyroX =(((((AdcRegs.ADCRESULT2>>4) ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)
     /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI ;//theta_dot_3 pitch
	  *_GyroY=(((((AdcRegs.ADCRESULT1>>4) /*- ZOFFSET/3*/ ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)
    /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI ;//theta_dot_1 roll
	  *_GyroZ=(((((AdcRegs.ADCRESULT3>>4) ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)
    /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI ;//theta_dot_2 yaw;  
    
}    
 
void CRS0304_readGyroRaw1(double *_GyroXYZ)
{
    CRS0304_readGyroRaw(_GyroXYZ,_GyroXYZ+1,_GyroXYZ+2);
}

void CRS0304_readGyroRawCal(double *_GyroX,double *_GyroY,double *_GyroZ)
{
   CRS0304_readGyroRaw(_GyroX,_GyroY,_GyroZ); 
   *_GyroX+=offsets[0]; 
   *_GyroY+=offsets[1];
   *_GyroZ+=offsets[2];
}
   

void MPU_getMotion6(float *ax,float *ay,float *az,float *gx,float *gy,float *gz)
{

      float x,y,z;
      gOffsets[0]=0.0;
      gOffsets[1]=0.0;
      gOffsets[2]=0.0;
      for(int i=0;i<totSamples;i++)
       {
     
	  CRS0304_readGyroRawCal(&x,&y,&z);

	  gOffsets[0]=(gOffsets[0]*i+x)/(i+1);
	  gOffsets[1]=(gOffsets[1]*i+x)/(i+1);
	  gOffsets[2]=(gOffsets[2]*i+x)/(i+1);
    }
      *ax = (((AdcRegs.ADCRESULT5>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_X_AXIS)*10;
      *ay = (((AdcRegs.ADCRESULT6>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Y_AXIS)*10;
      *az = (((AdcRegs.ADCRESULT4>>4)*MAX_AD_VOLTAGE/ADC_SCALE)-VOLTGEZERO_Z_AXIS)*10;

      *gx =(((((AdcRegs.ADCRESULT2>>4) ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)
     /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI- gOffsets[0];//theta_dot_3 pitch
	  *gy=(((((AdcRegs.ADCRESULT1>>4) /*- ZOFFSET/3*/ ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)
    /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI - gOffsets[1] ;//theta_dot_1 roll
	  *gz=(((((AdcRegs.ADCRESULT3>>4) ) * MAX_AD_VOLTAGE / ADC_SCALE) * 20.0 - 5.0 * VDD)
    /( 2.0 * VDD * SCALE_FACTOR) )/ 180 * PI - gOffsets[2];//theta_dot_2 yaw;
}

